#include "car.hpp"

Car::Car(){
    carModel.setBias(-1.0f, 0.0f, -0.2f);
}

void Car::Step(float throttle, float steering, float timedelta){
    if (!isBreakingForward && throttle < -0.1f && speed > 0.1f){
        isBreakingForward = true;
    }
    else if(isBreakingForward && throttle > -0.01f){
        isBreakingForward = false;
    }
    if (!isBreakingBackward && throttle > 0.1f && speed < -0.1f){
        isBreakingBackward = true;
    }
    else if(isBreakingBackward && throttle < 0.01f){
        isBreakingBackward = false;
    }
    float acc = throttle * 1e+1;

    if(isBreakingForward) acc = (speed > 0.1f) ? (acc * 4) : (0.0f);
    if(isBreakingBackward) acc = (speed < -0.1f) ? (acc * 4) : (0.0f);

    speed += acc * timedelta;
    speed -= kd * speed * timedelta;

    if((isBreakingForward || isBreakingBackward) && abs(speed) <= 0.1f) speed = 0.0f;

    frontAngle += steering * speed * 1e-3;
    velocity[0] = speed * -cos(frontAngle);
    velocity[2] = speed * sin(frontAngle);
    position[0] += velocity[0] * timedelta;
    position[2] += velocity[2] * timedelta;
}

void Car::Draw(){
    glPushMatrix();
    glTranslatef(position[0], position[1], position[2]);
    glRotatef(frontAngle * 180 / acos(-1), 0.0f, 1.0f, 0.0f);
    carModel.Draw();
    glPopMatrix();
}

vector<float> Car::forwardVector(){
    vector<float> forward;
    forward.push_back(-cos(frontAngle));
    forward.push_back(0.0f);
    forward.push_back(sin(frontAngle));
    return forward;
}

vector<float> Car::rightVector(){
    vector<float> right;
    right.push_back(sin(frontAngle));
    right.push_back(0.0f);
    right.push_back(cos(frontAngle));
    return right;
}

fPoint Car::getfeaturePoint(int i){
    fPoint point;
	vector<float> forward = forwardVector();
	vector<float> right = rightVector();
    point.x = position[0] + forward[0] * featurePoints[i][0] + right[0] * featurePoints[i][2];
    point.y = position[2] + forward[2] * featurePoints[i][0] + right[2] * featurePoints[i][2];
    return point;
}

vector<fPoint> Car::getfeaturePoints(){
    vector<fPoint> points;
	for(unsigned int i=0; i<featurePoints.size(); i++){
		points.push_back(getfeaturePoint(i));
	}
    return points;
}

void Car::reset(){
    position = {0, 0, 0};
    speed = 0;
    frontAngle = 0;
}
